#include "TCBRotInterpolator.h"

//catmull-rom splines
void TCBRotInterpolator::GetVectors(float time, float &normalizedTime, Quat &pt1, Quat &pt2, Quat &tn1, Quat &tn2)
{
	//key on right side
	std::vector<RotKey*>::iterator rk;

	for (rk = keys.begin(); rk != keys.end(); rk++)
		if ((*rk) ->time >= time)
			break;
	
	//time normalization
	normalizedTime = (time - (*(rk - 1)) ->time) / ((*rk) ->time - (*(rk - 1)) ->time);

	pt1 = (*(rk - 1)) ->value;
	pt2 = (*rk) ->value;

	if ((rk - 1) != keys.begin())
		tn1 = (pt2 - (*(rk - 2)) ->value) * 0.5f;
	else
		tn1 = (pt2 - pt1) * 1.5f;

	if ((rk + 1) != keys.end())
		tn2 = ((*(rk + 1)) ->value - pt1) * 0.5f;
	else
		tn2 = ((*rk) ->value - (*(rk - 1)) ->value) * 1.5f;

	if (keys.size() == 2)
		tn1 = tn2 = pt2 - pt1;
	else
	{
		if ((rk - 1) == keys.begin())
			tn1 = tn1 - (tn2 * 0.5f);

		if ((rk + 1) == keys.end())
			tn2 = tn2 - (tn1 * 0.5f);
	}
}

void TCBRotInterpolator::GetValue(float time, float &angle, Vec3 &axis)
{
	if (keys.size() == 0)
		return;

	if (time <= (*keys.begin()) ->time)
	{
		(*keys.begin()) ->value.QuatToRotate(angle, axis);
		return;
	}

	if (time >= (*(keys.end() - 1)) ->time)
	{
		(*(keys.end() - 1)) ->value.QuatToRotate(angle, axis);
		return;
	}

	Quat pt1;
	Quat pt2;
	Quat tn1;
	Quat tn2;
	float normTime;

	GetVectors(time, normTime, pt1, pt2, tn1, tn2);

	float t1 = normTime;
	float t2 = t1 * normTime;
	float t3 = t2 * normTime;

	Quat value =
		(pt1 * (2.0f * t3 - 3.0f * t2 + 1)) +
		(pt2 * (-2.0f * t3 + 3.0f * t2)) +
		(tn1 * (t3 - 2.0f * t2 + t1)) +
		(tn2 * (t3 - t2));

	value.Normalize();
	value.QuatToRotate(angle, axis);
}

void TCBRotInterpolator::AddKey(float time, float angle, Vec3 axis)
{
	RotKey *key = new RotKey();
	key ->time = time;
	key ->value.RotateToQuat(angle, axis);

	std::vector<RotKey*>::iterator i;
	for (i = keys.begin(); i != keys.end(); i++)
		if ((*i) ->time >= key ->time)
			break;

	keys.insert(i, key);
}